#include <iostream>
#include <chrono>
#include "RoomExplorationActionGoal.h"
#include "boustrophedon_explorator.h"

using namespace std;

IplImage *remove_noise_points(std::string image_path)
{
	CvSeq *contour = NULL;
	double minarea = 200;
	double tmparea = 0.0;

	// IplImage *img_src = cvLoadImage("/home/zy/Desktop/CCPP_backup/boustrophedo/map_supermarket.png", CV_LOAD_IMAGE_GRAYSCALE);
	IplImage *img_src = cvLoadImage(image_path.c_str(), CV_LOAD_IMAGE_GRAYSCALE);
	IplImage *img_Clone = cvCloneImage(img_src);
	//访问二值图像每个点的值
	uchar *pp;
	//显示原始图像
	// cvNamedWindow("img_src", CV_WINDOW_AUTOSIZE);
	// cvShowImage("img_src", img_src);

	IplImage *img_dst = cvCreateImage(cvGetSize(img_src), IPL_DEPTH_8U, 1);

	//------------搜索二值图中的轮廓，并从轮廓树中删除面积小于某个阈值minarea的轮廓-------------//
	CvScalar color = cvScalar(255, 0, 0); //CV_RGB(128,0,0);
	CvContourScanner scanner = NULL;
	CvMemStorage *storage = cvCreateMemStorage(0);
	scanner = cvStartFindContours(img_src, storage, sizeof(CvContour), CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE, cvPoint(0, 0));
	//开始遍历轮廓树
	CvRect rect;
	while (contour = cvFindNextContour(scanner))
	{
		tmparea = fabs(cvContourArea(contour));
		rect = cvBoundingRect(contour, 0);
		if (tmparea < minarea /*||tmparea>4900*/)
		{

			//当连通域的中心点为黑色时，而且面积较小则用白色进行填充
			pp = (uchar *)(img_Clone->imageData + img_Clone->widthStep * (rect.y + rect.height / 2) + rect.x + rect.width / 2);
			if (pp[0] == 0)
			{
				for (int y = rect.y; y < rect.y + rect.height; y++)
				{
					for (int x = rect.x; x < rect.x + rect.width; x++)
					{
						pp = (uchar *)(img_Clone->imageData + img_Clone->widthStep * y + x);

						if (pp[0] == 0)
						{
							pp[0] = 255;
						}
					}
				}
			}
		}
	}

	return img_Clone;
	// cvSaveImage("/home/zy/Desktop/CCPP_backup/boustrophedo/remove_noise_supermarket.png", img_Clone);
}

bool removeUnconnectedRoomParts(cv::Mat &room_map)
{
	// create new map with segments labeled by increasing labels from 1,2,3,...
	cv::Mat room_map_int(room_map.rows, room_map.cols, CV_32SC1);
	for (int v = 0; v < room_map.rows; ++v)
	{
		for (int u = 0; u < room_map.cols; ++u)
		{
			if (room_map.at<uchar>(v, u) == 255)
				room_map_int.at<int32_t>(v, u) = -100;
			else
				room_map_int.at<int32_t>(v, u) = 0;
		}
	}

	std::map<int, int> area_to_label_map; // maps area=number of segment pixels (keys) to the respective label (value)
	int label = 1;
	for (int v = 0; v < room_map_int.rows; ++v)
	{
		for (int u = 0; u < room_map_int.cols; ++u)
		{
			if (room_map_int.at<int32_t>(v, u) == -100)
			{
				const int area = cv::floodFill(room_map_int, cv::Point(u, v), cv::Scalar(label), 0, 0, 0, 8 | cv::FLOODFILL_FIXED_RANGE);
				area_to_label_map[area] = label;
				++label;
			}
		}
	}
	// abort if area_to_label_map.size() is empty
	if (area_to_label_map.size() == 0)
		return false;

	// remove all room pixels from room_map which are not accessible
	const int label_of_biggest_room = area_to_label_map.rbegin()->second;
	std::cout << "label_of_biggest_room=" << label_of_biggest_room << std::endl;
	for (int v = 0; v < room_map.rows; ++v)
		for (int u = 0; u < room_map.cols; ++u)
			if (room_map_int.at<int32_t>(v, u) != label_of_biggest_room)
				room_map.at<uchar>(v, u) = 0;

	return true;
}

// 读取地图
ipa_building_msgs::RoomExplorationActionGoal Generate_goal()
{
	// 解析地图
	std::string image_path;

	// 二楼办公室
	// image_path = "/home/zy/Desktop/CCPP_backup/boustrophedo/vslam_map_-1_grid.png";

	// 一楼商场
	image_path = "/home/zy/Desktop/CCPP_backup/boustrophedo/map_supermarket.png";
	// 去除噪点
	IplImage *image_remove_noise = remove_noise_points(image_path);

	cv::Mat map_raw = cv::cvarrToMat(image_remove_noise);

	// double scale = 0.25;
	// // string imageurl = "F:/opencv/samples/cpp/baboon.jpg";
	// // Mat image =imread(imageurl);
	// cv::Size dsize = cv::Size(map_raw.cols * scale, map_raw.rows * scale);
	// cv::Mat image_scale = cv::Mat(dsize, 0);
	// cv::resize(map_raw, image_scale, dsize);

	// // auto resImg1 = cv::resize(map, (300,300), interpolation=cv::INTER_CUBIC);
	// std::cout << "map_raw.rows  " << map_raw.rows << "  map_raw.cols  " << map_raw.cols << std::endl;
	// std::cout << "image_scale.rows  " << image_scale.rows << "  image_scale.cols  " << image_scale.cols << std::endl;

	cv::Mat map = map_raw;

	// 一楼商场
	for (int y = 0; y < map.rows; y++)
	{
		for (int x = 0; x < map.cols; x++)
		{
			//find not reachable regions and make them black
			if (map.at<unsigned char>(y, x) == 255)
			{
				map.at<unsigned char>(y, x) = 255; // 白
			}
			//else make it white
			else
			{
				map.at<unsigned char>(y, x) = 0; // 黑
			}
		}
	}
	std::cout << "map-size: " << map.rows << "x" << map.cols << std::endl;

	//save image
	// imwrite("/home/zy/Desktop/CCPP_backup/boustrophedo/map.png", map);

	cv::Mat labeling = map;
	// sensor_msgs::Image labeling;
	// cv_bridge::CvImage cv_image;
	// //	cv_image.header.stamp = ros::Time::now();
	// cv_image.encoding = "mono8";
	// cv_image.image = map;
	// cv_image.toImageMsg(labeling);

	std::vector<double> origin(3, 0); // (0,0,0)
	ipa_building_msgs::Point_x_y_z map_origin;
	// map_origin.x = origin[0];
	// map_origin.y = origin[1];
	// map_origin.z = origin[2];
	map_origin.x = 0;
	map_origin.y = 0;
	map_origin.z = 0;

	std::vector<double> start_pos = {0, 0, 0};
	ipa_building_msgs::Point_x_y_theta starting_position;
	starting_position.x = start_pos[0];
	starting_position.y = start_pos[1];
	starting_position.theta = start_pos[2];

	std::vector<ipa_building_msgs::Point_x_y> fov_points(4);
	fov_points[0].x = 0.04035; // this field of view represents the off-center iMop floor wiping device
	fov_points[0].y = -0.136;
	fov_points[1].x = 0.04035;
	fov_points[1].y = 0.364;
	fov_points[2].x = 0.54035; // todo: this definition is mirrored on x (y-coordinates are inverted) to work properly --> check why, make it work the intuitive way
	fov_points[2].y = 0.364;
	fov_points[3].x = 0.54035;
	fov_points[3].y = -0.136;
	int planning_mode = 2; // viewpoint planning

	ipa_building_msgs::Point_x_y fov_origin;
	fov_origin.x = 0.;
	fov_origin.y = 0.;

	// 地图的分辨率 m
	double resolution = 0.05;
	// 机器人的半径 m
	double robot_radius = 1;
	// 进三退二规划器中每一个间隔的距离 m
	double grid_spacing_in_meter_ = 0.5;
	// 地图主清扫方向和x轴的夹角
	double theta_ccpp_x_ = -0.359602;

	//
	double coverage_radius = 1.0;
	// 最小的清扫距离
	float min_line_cleaning_distance_ = 3;
	// 补充轨迹的距离 m
	float complementary_path_distance_ = 2;
	// 需要Hybrid A* 的距离 m
	float need_hybrid_distance_ = 5;
	// 距离边界的距离 m
	float distance_to_boundary = 1;

	// 分割cell的最小面积 m^2
	double min_cell_area_ = 20;

	// 组帧为goal的数据结构
	ipa_building_msgs::RoomExplorationActionGoal goal;
	goal.input_map = labeling;
	goal.map_resolution = resolution;
	goal.map_origin = map_origin;
	goal.robot_radius = robot_radius; // turtlebot, used for sim 0.177, 0.4
	goal.coverage_radius = coverage_radius;
	goal.field_of_view = fov_points;
	goal.field_of_view_origin = fov_origin;
	goal.starting_position = starting_position;
	goal.planning_mode = planning_mode;
	goal.min_line_cleaning_distance = min_line_cleaning_distance_;
	goal.complementary_path_index = complementary_path_distance_ / resolution;
	goal.need_hybrid_index = need_hybrid_distance_ / resolution;
	goal.index_to_boundary = distance_to_boundary / resolution;
	goal.min_cell_area = min_cell_area_;
	goal.grid_spacing_in_meter = grid_spacing_in_meter_;
	return goal;
}

bool explore_room(ipa_building_msgs::RoomExplorationActionGoal goal)
{

	cout << ("start of explore_room   ") << endl;

	//  I. read the given parameters out of the goal
	// todo: this is only correct if the map is not rotated
	const cv::Point2d map_origin(goal.map_origin.x, goal.map_origin.y);
	const float map_resolution = goal.map_resolution; // in [m/cell]
	const float map_resolution_inverse = 1. / map_resolution;
	const float min_line_cleaning_distance = goal.min_line_cleaning_distance;
	cout << "map origin: " << map_origin << " m      map resolution: " << map_resolution << " m/cell" << endl;

	const float robot_radius = goal.robot_radius;
	const int robot_radius_in_pixel = (robot_radius / map_resolution);
	cout << "robot radius: " << robot_radius << " m   (" << robot_radius_in_pixel << " px)" << endl;

	const cv::Point starting_position((goal.starting_position.x - map_origin.x) / map_resolution,
									  (goal.starting_position.y - map_origin.y) / map_resolution);
	cout << "starting point: (" << goal.starting_position.x << ", " << goal.starting_position.y
		 << ") m   (" << starting_position << " px)" << std::endl;

	int PLAN_FOR_FOOTPRINT = 1;
	int PLAN_FOR_FOV = 2;
	int planning_mode_ = goal.planning_mode;
	if (planning_mode_ == PLAN_FOR_FOOTPRINT)
		std::cout << "planning mode: planning coverage path with robot's footprint" << std::endl;
	else if (planning_mode_ == PLAN_FOR_FOV)
		std::cout << "planning mode: planning coverage path with robot's field of view" << std::endl;

	// 接受地图
	cv::Mat room_map = goal.input_map;

	// 场地的面积
	int area_px = 0; // room area in pixels
	for (int v = 0; v < room_map.rows; ++v)
		for (int u = 0; u < room_map.cols; ++u)
			if (room_map.at<uchar>(v, u) >= 250)
				area_px++;
	cout << "room area = " << area_px * map_resolution * map_resolution << " m^2" << endl;

	// closing operation to neglect inaccessible areas and map errors/artifacts
	// 腐蚀与膨胀(Eroding and Dilating)
	int map_correction_closing_neighborhood_size_ = 10;
	cv::Mat temp;
	cv::erode(room_map, temp, cv::Mat(), cv::Point(-1, -1), map_correction_closing_neighborhood_size_);
	cv::dilate(temp, room_map, cv::Mat(), cv::Point(-1, -1), map_correction_closing_neighborhood_size_);

	// remove unconnected, i.e. inaccessible, parts of the room
	// (i.e. obstructed by furniture), only keep the room with the largest area
	const bool room_not_empty = removeUnconnectedRoomParts(room_map);
	if (room_not_empty == false)
	{
		cout << "the requested room is too small for generating exploration trajectories." << endl;
		return false;
	}

	// get the grid size, to check the areas that should be revisited later
	double grid_spacing_in_meter = 0.0; // is the square grid cell side length that fits into the circle with the robot's coverage radius or fov coverage radius
	float fitting_circle_radius_in_meter = 0;
	Eigen::Matrix<float, 2, 1> fitting_circle_center_point_in_meter; // this is also considered the center of the field of view, because around this point the maximum radius incircle can be found that is still inside the fov
	std::vector<Eigen::Matrix<float, 2, 1>> fov_corners_meter(4);
	const double fov_resolution = 1000; // in [cell/meter]
	if (planning_mode_ == PLAN_FOR_FOV) // read out the given fov-vectors, if needed
	{
		// Get the size of one grid cell s.t. the grid can be completely covered by the field of view (fov) from all rotations around it.
		for (int i = 0; i < 4; ++i)
			fov_corners_meter[i] << goal.field_of_view[i].x, goal.field_of_view[i].y;
		computeFOVCenterAndRadius(fov_corners_meter, fitting_circle_radius_in_meter, fitting_circle_center_point_in_meter, fov_resolution);
		// get the edge length of the grid square that fits into the fitting_circle_radius
		grid_spacing_in_meter = fitting_circle_radius_in_meter * std::sqrt(2);
	}
	else // if planning should be done for the footprint, read out the given coverage radius
	{
		grid_spacing_in_meter = goal.coverage_radius * std::sqrt(2);
	}

	// map the grid size to an int in pixel coordinates, using floor method
	grid_spacing_in_meter = goal.grid_spacing_in_meter;
	const double grid_spacing_in_pixel = grid_spacing_in_meter / map_resolution; // is the square grid cell side length that fits into the circle with the robot's coverage radius or fov coverage radius, multiply with sqrt(2) to receive the whole working width
	std::cout << "grid size: " << grid_spacing_in_meter << " m   (" << grid_spacing_in_pixel << " px)" << std::endl;
	// set the cell_size_ for #4 convexSPP explorator or #5 flowNetwork explorator if it is not provided
	// if (cell_size_ <= 0)
	// 	cell_size_ = std::floor(grid_spacing_in_pixel);

	// II. plan the path using the wanted planner
	// todo: consider option to provide the inflated map or the robot radius to the functions instead of inflating with half cell size there
	Eigen::Matrix<float, 2, 1> zero_vector;
	zero_vector << 0, 0;
	std::vector<ipa_building_msgs::Point_x_y_theta> exploration_path;

	// int room_exploration_algorithm_ = 8;

	double grid_obstacle_offset_ = 0;
	double path_eps_ = 1; // distance between two points when generating the (line) path,[pixel], where one pixel is the size of one cell in the navigation grid map
	int cell_visiting_order_ = 1;
	double min_cell_area_ = goal.min_cell_area;
	int max_deviation_from_track_ = -1;
	int complementary_path_index_ = goal.complementary_path_index;
	int need_hybrid_index_ = goal.need_hybrid_index;
	int index_to_boundary_ = goal.index_to_boundary;

	BoustrophedonExplorer BoustrophedonExplorer_;

	// boustrophedon_explorator
	BoustrophedonExplorer_.getExplorationPath(room_map, exploration_path, map_resolution,
											  starting_position, map_origin,
											  grid_spacing_in_pixel,
											  grid_obstacle_offset_, path_eps_, cell_visiting_order_,
											  false, fitting_circle_center_point_in_meter,
											  min_cell_area_, max_deviation_from_track_,
											  min_line_cleaning_distance, complementary_path_index_,
											  need_hybrid_index_, index_to_boundary_

	);

	return false;
}

int main()
{
	std::chrono::system_clock::system_clock::time_point start_time =
		std::chrono::system_clock::system_clock::now();

	// 读取地图
	ipa_building_msgs::RoomExplorationActionGoal goal_ = Generate_goal();

	bool CCPP = explore_room(goal_);

	std::chrono::system_clock::system_clock::time_point end_time =
		std::chrono::system_clock::system_clock::now();

	std::chrono::duration<double> elapsed_seconds = end_time - start_time;
	std::cout << "time spent:" << elapsed_seconds.count() << " s" << std::endl;

	return 0;
}